/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-06 09:51:41
 * @LastEditors: EltonLi lijiaming@uavi-tech.com
 * @LastEditTime: 2023-09-11 16:45:32
 * @FilePath: \zino-fc-v4\ZINO\sensor\QMI8658\qmi8658.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-09-06     UAVI02       the first version
 */
#include "qmi8658.h"
#include "zino.h"
#include <drv_spi.h>
#include <rtthread.h>

#define DBG_TAG "QMI8658"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define QMI8658_DEVICE_NAME "qmi8658"

static rt_err_t qmi8658_spi_init(qmi8658_device_t *dev)
{
    dev->spi_dev = (struct rt_spi_device *)rt_device_find(QMI8658_DEVICE_NAME);
    if (dev->spi_dev != RT_NULL)
    {
        dev->spi_dev->config.max_hz = 10000000;
        dev->spi_dev->config.data_width = 8;
        dev->spi_dev->config.mode = RT_SPI_MODE_0 | RT_SPI_MSB;
        LOG_D("%s spi init ok!", QMI8658_DEVICE_NAME);
        return RT_EOK;
    }
    else
    {
        LOG_E("init failed! can't find %s spi!", QMI8658_DEVICE_NAME);
        return -RT_ERROR;
    }
}

static rt_err_t qmi8658_spi_write_reg(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t *data)
{
    return rt_spi_send_then_send(dev->spi_dev, &reg_addr, 1, data, 1);
}

// static rt_err_t qmi8658_spi_write_regs(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t *data, uint8_t len)
// {
//     return rt_spi_send_then_send(dev->spi_dev, &reg_addr, 1, data, len);
// }

// static rt_err_t qmi8658_spi_read_reg(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t *data)
// {
//     reg_addr |= 0x80;
//     return rt_spi_send_then_recv(dev->spi_dev, &reg_addr, 1, data, 1);
// }

static rt_err_t qmi8658_spi_read_regs(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t *data, uint8_t len)
{
    reg_addr |= 0x80;
    return rt_spi_send_then_recv(dev->spi_dev, &reg_addr, 1, data, len);
}

static rt_err_t qmi8658_spi_write_bits(qmi8658_device_t *dev, rt_uint8_t reg, rt_uint8_t start_bit, rt_uint8_t len, rt_uint8_t data)
{
    rt_uint8_t byte, mask;
    rt_err_t res = RT_EOK;

    res = qmi8658_spi_read_regs(dev, reg, &byte, 1);
    if (res != RT_EOK)
    {
        return res;
    }
    mask = ((1 << len) - 1) << (start_bit - len + 1);
    data <<= (start_bit - len + 1); // shift data into correct position
    data &= mask;                   // zero all non-important bits in data
    byte &= ~(mask);                // zero all important bits in existing byte
    byte |= data;
    return qmi8658_spi_write_reg(dev, reg, &byte);
}

static rt_err_t qmi8658_spi_set_reg(qmi8658_device_t *dev, uint8_t cmd, uint16_t param)
{
    rt_err_t res = RT_EOK;
    switch (cmd)
    {
    case QMI8658_ACC_ODR_SET:
        res = qmi8658_spi_write_bits(dev, QMI8658_REG_CTRL2, 3, 4, param);
        break;
    case QMI8658_ACC_RANGE_SET:
        res = qmi8658_spi_write_bits(dev, QMI8658_REG_CTRL2, 6, 3, param);
        dev->range.acce_range = param;
        rt_kprintf("sen_acce = %d\n", dev->range.acce_range);
        break;

    case QMI8658_GYRO_ODR_SET:
        res = qmi8658_spi_write_bits(dev, QMI8658_REG_CTRL3, 3, 4, param);
        break;
    case QMI8658_GYRO_RANGE_SET:
        res = qmi8658_spi_write_bits(dev, QMI8658_REG_CTRL3, 6, 4, param);
        dev->range.gyro_range = param;
        rt_kprintf("sen_gyro = %d\n", dev->range.gyro_range);
        break;
    case QMI8658_ALPF_SET:
        res = qmi8658_spi_write_bits(dev, QMI8658_REG_CTRL5, 4, 1, 1);
        res = qmi8658_spi_write_bits(dev, QMI8658_REG_CTRL5, 6, 2, param);
        break;
    case QMI8658_GLPF_SET:
        res = qmi8658_spi_write_bits(dev, QMI8658_REG_CTRL5, 0, 1, 1);
        res = qmi8658_spi_write_bits(dev, QMI8658_REG_CTRL5, 2, 2, param);
        break;
    case QMI8658_MODE_SET:
        res = qmi8658_spi_write_reg(dev, QMI8658_REG_CTRL7, param);
        break;
    default:
        break;
    }
    return res;
}

static rt_err_t qmi8658_init(qmi8658_device_t *dev, const char *bus_name)
{
    rt_uint8_t res = RT_EOK;
    res = qmi8658_spi_init(dev);
    if (res != RT_EOK)
    {
        LOG_E("qmi8658 find spi device failed");
        return -RT_ERROR;
    }
    qmi8658_spi_read_regs(dev, QMI8658_REG_WHO_AM_I, &dev->dev_id, 1);
    if (dev->dev_id != QMI8658_DEFAULT_ID)
    {
        LOG_E("qmi8658 read id error, rx: 0x%X, expect: 0x%X", dev->dev_id, QMI8658_DEFAULT_ID);
        return -RT_ERROR;
    }
    else
    {
        LOG_D("qmi8658 read id success, rx:0x%x, expect:0x%x", dev->dev_id, QMI8658_DEFAULT_ID);
        // res += qmi8658_spi_write_reg(dev, QMI8658_REG_CTRL1, 64);
        res += qmi8658_spi_write_reg(dev, QMI8658_REG_CTRL7, QMI8658_CTRL7_DisableSyncSmpleMode | QMI8658_CTRL7_HighSpeedInternalClock | QMI8658_CTRL7_GyroFullMode | QMI8658_CTRL7_DisalbeAttitudeEngine | QMI8658_CTRL7_DisableMagnetmoeter | QMI8658_CTRL7_EnableGyro | QMI8658_CTRL7_EnableAcc);
    }
    if (res == RT_EOK)
    {
        LOG_I("Device init succeed!");
    }
    else
    {
        LOG_E("Error in device initialization!");
    }
    return res;
}

static rt_err_t qmi8658_get_acc_raw(qmi8658_device_t *dev)
{
    rt_err_t res;
    rt_uint8_t buffer[6];
    res = qmi8658_spi_read_regs(dev, QMI8658_REG_AX_L, buffer, 6);
    if (res != RT_EOK)
        return res;
    dev->acce_raw.x = (buffer[1] << 8) | buffer[0];
    dev->acce_raw.y = (buffer[3] << 8) | buffer[2];
    dev->acce_raw.z = (buffer[5] << 8) | buffer[4];
    return RT_EOK;
}

static rt_err_t qmi8658_get_gyro_raw(qmi8658_device_t *dev)
{
    rt_err_t res;
    rt_uint8_t buffer[6];
    res = qmi8658_spi_read_regs(dev, QMI8658_REG_GX_L, buffer, 6);
    if (res != RT_EOK)
        return res;
    dev->gyro_raw.x = (buffer[1] << 8) | buffer[0];
    dev->gyro_raw.y = (buffer[3] << 8) | buffer[2];
    dev->gyro_raw.z = (buffer[5] << 8) | buffer[4];
    return RT_EOK;
}

static qmi8658_device_t *_qmi8658_init(struct rt_sensor_intf *intf)
{
    qmi8658_device_t *qmi8658 = RT_NULL;
    uint8_t res;
    qmi8658 = rt_calloc(1, sizeof(qmi8658_device_t));
    res = qmi8658_init(qmi8658, intf->dev_name);
    if (res == RT_EOK)
        return qmi8658;
    else
        return RT_NULL;
}

static rt_size_t qmi8658_fetch_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len)
{
    qmi8658_device_t *qmi8658 = (qmi8658_device_t *)sensor->parent.user_data;
    struct rt_sensor_data *data = (struct rt_sensor_data *)buf;
    if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
    {
        if (qmi8658_get_acc_raw(qmi8658) != RT_EOK)
            return 0;
        data->type = RT_SENSOR_CLASS_ACCE;
        data->data.acce.x = qmi8658->acce.x;
        data->data.acce.y = qmi8658->acce.y;
        data->data.acce.z = qmi8658->acce.z;
        data->timestamp = rt_sensor_get_ts();
    }
    else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
    {
        if (qmi8658_get_acc_raw(qmi8658) != RT_EOK)
            return 0;
        data->type = RT_SENSOR_CLASS_GYRO;
        data->data.gyro.x = qmi8658->gyro.x * 1000;
        data->data.gyro.y = qmi8658->gyro.y * 1000;
        data->data.gyro.z = qmi8658->gyro.z * 1000;
        data->timestamp = rt_sensor_get_ts();
    }
    return 1;
}

static rt_err_t qmi8658_control(struct rt_sensor_device *sensor, int cmd, void *args)
{
    rt_err_t res = RT_EOK;
    qmi8658_device_t *qmi8658 = sensor->parent.user_data;
    switch (cmd)
    {
    case RT_SENSOR_CTRL_SET_ODR:
        if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
        {
            res = qmi8658_spi_set_reg(qmi8658, QMI8658_ACC_ODR_SET, (rt_uint32_t)args);
        }
        else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
        {
            res = qmi8658_spi_set_reg(qmi8658, QMI8658_GYRO_ODR_SET, (rt_uint32_t)args);
        }
        break;
    case RT_SENSOR_CTRL_SET_RANGE:
        if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
        {
            res = qmi8658_spi_set_reg(qmi8658, QMI8658_ACC_RANGE_SET, (rt_uint32_t)args);
        }
        else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
        {
            res = qmi8658_spi_set_reg(qmi8658, QMI8658_GYRO_RANGE_SET, (rt_uint32_t)args);
        }
        break;
    default:
        break;
    }
    return res;
}

static struct rt_sensor_ops qmi8658_ops =
    {
        qmi8658_fetch_data,
        qmi8658_control};

rt_err_t rt_hw_qmi8658_init(const char *name, struct rt_sensor_config *cfg)
{
    rt_uint8_t res = RT_EOK;
    rt_sensor_t qmi_acce = RT_NULL, qmi_gyro = RT_NULL;
    qmi8658_device_t *QMI8658 = _qmi8658_init(&cfg->intf);
    if (QMI8658 == RT_NULL)
    {
        LOG_E("_qmi8658 init err!");
        return -RT_ERROR;
    }
    qmi_acce = rt_calloc(1, sizeof(struct rt_sensor_device));
    if (qmi_acce == RT_NULL)
        return -RT_ERROR;
    qmi_acce->info.type = RT_SENSOR_CLASS_ACCE;
    qmi_acce->info.vendor = RT_SENSOR_VENDOR_STM;
    qmi_acce->info.model = "qmi8658_acc";
    qmi_acce->info.unit = RT_SENSOR_UNIT_MG;
    qmi_acce->info.intf_type = RT_SENSOR_INTF_SPI;
    qmi_acce->info.period_min = 10;
    qmi_acce->info.fifo_max = 0;

    rt_memcpy(&qmi_acce->config, cfg, sizeof(struct rt_sensor_config));
    qmi_acce->ops = &qmi8658_ops;
    res = rt_hw_sensor_register(qmi_acce, name, RT_DEVICE_FLAG_RDWR, QMI8658);
    if (res != RT_EOK)
    {
        LOG_E("device register err code: %d", res);
        rt_free(qmi_acce);
        return -RT_ERROR;
    }
    LOG_I("lsm6dsr acc device init success");

    qmi_gyro = rt_calloc(1, sizeof(struct rt_sensor_device));
    if (qmi_gyro == RT_NULL)
        return -RT_ERROR;

    qmi_gyro->info.type = RT_SENSOR_CLASS_GYRO;
    qmi_gyro->info.vendor = RT_SENSOR_VENDOR_STM;
    qmi_gyro->info.model = "qmi8658_gyro";
    qmi_gyro->info.unit = RT_SENSOR_UNIT_MDPS;
    qmi_gyro->info.intf_type = RT_SENSOR_INTF_SPI;
    qmi_gyro->info.period_min = 10;
    qmi_gyro->info.fifo_max = 0;
    rt_memcpy(&qmi_gyro->config, cfg, sizeof(struct rt_sensor_config));
    qmi_gyro->ops = &qmi8658_ops;
    res = rt_hw_sensor_register(qmi_gyro, name, RT_DEVICE_FLAG_RDWR, QMI8658);
    if (res != RT_EOK)
    {
        LOG_E("device register err code: %d", res);
        rt_free(qmi_gyro);
        return -RT_ERROR;
    }
    LOG_I("qmi8658 gyro device success");
    return RT_EOK;
}

int rt_hw_qmi8658_port(void)
{
    struct rt_sensor_config cfg;
    cfg.intf.dev_name = QMI8658_DEVICE_NAME;
    cfg.intf.type = RT_Device_Class_SPIBUS;
    // cfg.intf.user_data = ;
    cfg.irq_pin.pin = RT_PIN_NONE;
    return rt_hw_qmi8658_init("qmi", &cfg);
}

// ZINO_APP_EXPORT(rt_hw_qmi8658_port);

void rt_hw_qmi8658_data(void)
{
    rt_device_t qmi_acce = RT_NULL, qmi_gyro = RT_NULL;
    struct rt_sensor_data qmi_acce_data, qmi_gyro_data;
    qmi_acce = rt_device_find("acce_qmi");
    qmi_gyro = rt_device_find("gyro_qmi");
    rt_device_open(qmi_acce, RT_DEVICE_FLAG_RDWR);
    rt_device_open(qmi_gyro, RT_DEVICE_FLAG_RDWR);
    rt_device_control(qmi_acce, RT_SENSOR_CTRL_SET_RANGE, QMI8658_CTRL2_ACC_RANGE_4G);
    rt_device_control(qmi_gyro, RT_SENSOR_CTRL_SET_RANGE, QMI8658_GYRO_RANGE_256dps);
    rt_device_control(qmi_acce, RT_SENSOR_CTRL_SET_ODR, QMI8658_ACC_ODR_NORMAL_1000Hz);
    rt_device_control(qmi_gyro, RT_SENSOR_CTRL_SET_ODR, QMI8658_GYRO_ODR_NORMAL_1000Hz);
    while (1)
    {
        rt_device_read(qmi_acce, 0, &qmi_acce_data, 1);
        rt_device_read(qmi_gyro, 0, &qmi_gyro_data, 1);
        rt_kprintf("acce: x:%5d, y:%5d, z:%5d \t", qmi_acce_data.data.acce.x, qmi_acce_data.data.acce.y, qmi_acce_data.data.acce.z);
        rt_kprintf("gyro: x:%5d, y:%5d, z:%5d, timestamp:%5d\n", qmi_gyro_data.data.gyro.x, qmi_gyro_data.data.gyro.y, qmi_gyro_data.data.gyro.z, qmi_gyro_data.timestamp);
        rt_thread_mdelay(10);
    }
}
MSH_CMD_EXPORT(rt_hw_qmi8658_data, get qmi8658 data);